#include "math_func.h"



LPF_TypeDef x_pos,y_pos;

void FilterParameter_Init(void)
{
	x_pos.kp = 0.1;
	x_pos.deathzoom = 1.0;
	x_pos.input = 0;
	x_pos.output = 0;
	
	y_pos.kp = 0.1;
	y_pos.deathzoom = 1.0;
	y_pos.input = 0;
	y_pos.output = 0;
		
}


void LPF_Calculate(LPF_TypeDef *filter)
{
//	float max,min;
//	max = filter->output + filter->deathzoom;
//	min = filter->output - filter->deathzoom;
//		
//	if(filter->input > max || filter->input < min)
//	{
//		return;
//	}	
	filter->output = filter->input * filter->kp + (1-filter->kp) * filter->output;
}


